#include "transformObject.h"

#include <math.h>
#include <iostream>

#include <GL/glew.h>

using namespace nv;

TransformObject::TransformObject() {

	m_transform.make_identity();
	m_rotation.set_value(m_transform);

}

void TransformObject::multToGL() {

	glMultMatrixf(m_transform._array);
}

void TransformObject::move(const vec3f &dPos) {

	m_position.x -= m_transform(0) * dPos.x + m_transform(1) * dPos.y
			+ m_transform(2) * dPos.z;
	m_position.y -= m_transform(4) * dPos.x + m_transform(5) * dPos.y
			+ m_transform(6) * dPos.z;
	m_position.z -= m_transform(8) * dPos.x + m_transform(9) * dPos.y
			+ m_transform(10) * dPos.z;

	//std::cout << m_position.x << " " << m_position.y << " " << m_position.z << " " << std::endl;

	m_transform(0 + 12) = -(m_transform(0) * m_position.x + m_transform(4)
			* m_position.y + m_transform(8) * m_position.z);
	m_transform(1 + 12) = -(m_transform(1) * m_position.x + m_transform(5)
			* m_position.y + m_transform(9) * m_position.z);
	m_transform(2 + 12) = -(m_transform(2) * m_position.x + m_transform(6)
			* m_position.y + m_transform(10) * m_position.z);
}

void TransformObject::rotate(float headingDegrees, float pitchDegrees) {


	float currpitch = getPitch();
	float pitchRange = M_PI*0.35f;

	//std::cout << pitchDegrees << std::endl;

	float tmp = currpitch - pitchDegrees;

	if(tmp > pitchRange && pitchDegrees < 0.f)
		pitchDegrees = currpitch - pitchRange;

	if(tmp < -pitchRange && pitchDegrees > 0.f)
		pitchDegrees = -currpitch - pitchRange;


	// Rotate camera about the world y axis.
	if (headingDegrees != 0.0f) {
		quatf rot(vec3f(0.0f, 1.0f, 0.0f), headingDegrees);
		m_rotation = normalize(rot * m_rotation);
	}

	// Rotate camera about its local x axis.
	if (pitchDegrees != 0.0f) {

			quatf rot(vec3f(1.0f, 0.0f, 0.0f), pitchDegrees);
			m_rotation = m_rotation * rot;

	}

	m_rotation = normalize(m_rotation);

	m_transform.set_value(m_rotation);

	m_transform(0 + 12) = -(m_transform(0) * m_position.x + m_transform(4)
			* m_position.y + m_transform(8) * m_position.z);
	m_transform(1 + 12) = -(m_transform(1) * m_position.x + m_transform(5)
			* m_position.y + m_transform(9) * m_position.z);
	m_transform(2 + 12) = -(m_transform(2) * m_position.x + m_transform(6)
			* m_position.y + m_transform(10) * m_position.z);

	//Utils::transposeGLMatrix(m_transform);
}

void TransformObject::Set(const vec3f &eye, const vec3f &target,
		const vec3f &up) {
	m_position = eye;

	vec3f m_zAxis(eye - target);
	m_zAxis = normalize(m_zAxis);

	vec3f m_xAxis(cross(up, m_zAxis));
	m_xAxis = normalize(m_xAxis);

	vec3f m_yAxis(cross(m_zAxis, m_xAxis));
	m_yAxis = normalize(m_yAxis);

	m_transform(0) = m_xAxis.x;
	m_transform(4) = m_xAxis.y;
	m_transform(8) = m_xAxis.z;
	m_transform(12) = -dot(m_xAxis, eye);

	m_transform(1) = m_yAxis.x;
	m_transform(5) = m_yAxis.y;
	m_transform(9) = m_yAxis.z;
	m_transform(13) = -dot(m_yAxis, eye);

	m_transform(2) = m_zAxis.x;
	m_transform(6) = m_zAxis.y;
	m_transform(10) = m_zAxis.z;
	m_transform(14) = -dot(m_zAxis, eye);

	// m_transform = transpose(m_transform);

	m_rotation.set_value(m_transform);
}

// return rotation around x axis from -90 to 90 deg
float TransformObject::getPitch(){


	float ang = atan2(2.f*(m_rotation.y*m_rotation.z +m_rotation.w*m_rotation.x),
			m_rotation.w*m_rotation.w - m_rotation.x*m_rotation.x -
			m_rotation.y*m_rotation.y + m_rotation.z*m_rotation.z);


	if(ang > 0.f && ang > M_PI*.5f)
		return M_PI - ang;
	else if(ang > -M_PI && ang < -M_PI*.5f)
		return -M_PI - ang;

	return -ang;

}

// return rotation around y axis from 0 to 360 deg
float TransformObject::getHeading(){

	float ang = asin(-2.f*(m_rotation.x*m_rotation.z - m_rotation.w*m_rotation.y));

	float pitchang = atan2(2.f*(m_rotation.y*m_rotation.z +m_rotation.w*m_rotation.x),
			m_rotation.w*m_rotation.w - m_rotation.x*m_rotation.x -
			m_rotation.y*m_rotation.y + m_rotation.z*m_rotation.z);


	if(fabs(pitchang) < M_PI*.5f){
		return M_PI - ang;
	}

	if(ang < 0.f)
		ang += M_PI*2.f;

	return ang;
}
/*

float TransformObject::getAxisRotation(const vec3f &axis, const vec3f &baseDir) {


	vec3f inPlaneDir = getPlaneDir(axis, baseDir);
	vec3f bDir = normalize(baseDir);



	vec3f c = cross(bDir, inPlaneDir);
    float angle = atan2(length(c), dot(bDir, inPlaneDir));
    return dot(c, axis) < 0.f ? -angle : angle;


}

vec3f TransformObject::getPlaneDir(const vec3f &axis, const vec3f &baseDir){

	vec3f tDir;

	m_rotation.mult_vec(baseDir, tDir);

	return normalize(tDir - axis * dot(tDir, axis));

}
*/


